; File   : fasmmath.asm
; License: LGPL

  IdentityMatrix4x4 dd 1.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0, 0.0,0.0,1.0,0.0, 0.0,0.0,0.0,1.0

; input : [[valueAddr]], [f1], [f2]
; output: [valueAddr]        
proc Clamp uses esi, valueAddr, min, max: DWORD
	mov    esi,[valueAddr]
	fld    [min]
	fld    dword ptr esi
	fcomp  st1
	fnstsw ax
	sahf
	jb     clampMin
	ffree  st
	fld    [max]
	fld    dword ptr esi
	fcomp  st1
	fnstsw ax
	sahf
	jbe    equal
	fstp   dword ptr esi
	jmp    endMain
  clampMin:
	fstp   dword ptr esi
	jmp    endMain
  equal:      
	ffree  st  
  endMain:
	ret
endp

; input : [x1], [y1], [x2], [y2]
; output: st
proc Angle uses eax, x1,y1,x2,y2: DWORD
	fld    [y1]
	fld    [y2]
	fsubp  st1,st
	fld    [x1]
	fld    [x2]
	fsubp  st1,st
	fpatan
	fldz
	fcomp  st1
	fnstsw ax
	sahf
	jae    finish
	fldpi
	push   2.0
	fld    dword ptr esp
	fmulp  st1,st
	fmulp  st1,st
	add    esp,4
  finish:
	ret
endp

; input : [f1], [f2]
; output: st0
proc MinFloat f1, f2: DWORD
	fld	[f1]
	fld	[f2]
	fcomi	st,st1
	fcmovnb st,st1
	ffree	st1
	ret
endp	 

; input : [f1], [f2]
; output: st
proc MaxFloat f1, f2: DWORD
	fld	[f1]
	fld	[f2]
	fcomi	st,st1
	fcmovb	st,st1
	ffree	st1
	ret
endp	

; input : [rad]
; output: st
proc Tan rad: DWORD
	fld   [rad]
	fptan
	fstp  st
	ret
endp

; input : [rad]
; output: st
proc Cot rad: DWORD
	fld   [rad]
	fptan
	fdivrp
	ret
endp

; input : [rad]
; output: [sinAddr], [cosAddr]
proc SinCos uses esi, sinAddr, cosAddr, rad: DWORD
	fld  [rad]
	fsincos
	mov  esi,[cosAddr]
	fstp dword ptr esi
	mov  esi,[sinAddr]
	fstp dword ptr esi
	ret
endp

; input : [rad], [radius]
; output: [sinAddr], [cosAddr]
proc SinCosRadius uses esi, sinAddr, cosAddr, rad, radius: DWORD
	fld  [rad]
	fsincos
	fmul [radius]
	mov  esi,[cosAddr]
	fstp dword ptr esi
	mov  esi,[sinAddr]
	fstp dword ptr esi
	ret
endp

; input : [x], [y]
; output: st
proc Arctan2 x, y: DWORD
	fld    [y]
	fld    [x]
	fpatan
	ret
endp

; ----- Vector3f functions -----

; input : [[vec]]
; output: [vec]
proc ZeroVector3f uses esi eax, vec: DWORD
	mov  esi,[vec]
	xor  eax,eax
	mov  [esi],eax
	mov  [esi+4],eax
	mov  [esi+8],eax
	ret
endp

; input : [[vec1]],[[vec2]]
; output: [vec]
proc AddVector3f uses eax ebx ecx, vec,vec1,vec2: DWORD
	mov  eax,[vec]
	mov  ebx,[vec1]
	mov  ecx,[vec2]
	fld  dword ptr ebx
	fadd dword ptr ecx
	fstp dword ptr eax
	fld  dword ptr ebx+4
	fadd dword ptr ecx+4
	fstp dword ptr eax+4
	fld  dword ptr ebx+8
	fadd dword ptr ecx+8
	fstp dword ptr eax+8 
	ret
endp

; input : [[vec1]],[[vec2]]
; output: [vec]
proc SubVector3f uses eax ebx ecx, vec,vec1,vec2: DWORD
	mov   eax,[vec]
	mov   ebx,[vec1]
	mov   ecx,[vec2]
	fld   dword ptr ebx
	fsub  dword ptr ecx
	fstp  dword ptr eax
	fld   dword ptr ebx+4
	fsub  dword ptr ecx+4
	fstp  dword ptr eax+4
	fld   dword ptr ebx+8
	fsub  dword ptr ecx+8
	fstp  dword ptr eax+8 
	ret
endp

; input : [[vec1]],[[vec2]]
; output: [vec]
proc CrossProduct3f uses eax ebx ecx, vec,vec1,vec2: DWORD
	mov   eax,[vec]
	mov   ebx,[vec1]
	mov   ecx,[vec2]
  ; (vec1.y*vec2.z) - (vec1.z*vec2.y) 
	fld   dword ptr ebx+4
	fmul  dword ptr ecx+8
	fld   dword ptr ebx+8
	fmul  dword ptr ecx+4
	fsubp st1,st
	fstp  dword ptr eax
  ; (vec1.z*vec2.x) - (vec1.x*vec2.z)  
	fld   dword ptr ebx+8
	fmul  dword ptr ecx
	fld   dword ptr ebx
	fmul  dword ptr ecx+8
	fsubp st1,st
	fstp  dword ptr eax+4	   
  ; (vec1.x*vec1.y) - (vec1.y*vec1.x)      
	fld   dword ptr ebx
	fmul  dword ptr ecx+4
	fld   dword ptr ebx+4
	fmul  dword ptr ecx
	fsubp st1,st
	fstp  dword ptr eax+8	   
	ret
endp

; input : [[vec]]
; output: st
proc Norm3f uses esi, vec: DWORD
	mov   esi,[vec]
	fld   dword ptr esi
	fmul  st,st
	fld   dword ptr esi+4
	fmul  st,st
	faddp 
	fld   dword ptr esi+8
	fmul  st,st
	faddp  
	ret
endp

; input : [[vec2]]
; output: [vec1]
proc Normalize3f uses edi esi eax, vec1, vec2: DWORD
	mov   edi,[vec1]
	mov   esi,[vec2]
	fld   dword ptr esi
	fmul  st,st
	fld   dword ptr esi+4
	fmul  st,st
	faddp 
	fld   dword ptr esi+8
	fmul  st,st
	faddp 
	
	fldz
	fcomp
	fnstsw ax
	sahf
	jz result
	fsqrt
	fld1
	fdivrp
  result:
	fld   st
	fmul  dword ptr esi	    
	fstp  dword ptr edi
	fld   st
	fmul  dword ptr esi+4
	fstp  dword ptr edi+4
	fmul  dword ptr esi+8
	fstp  dword ptr edi+8 
	ret
endp

; input : [[vec1]],[[vec2]]
; output: st
proc DotProduct3f uses esi edi, vec1, vec2: DWORD
	mov   esi,[vec1]
	mov   edi,[vec2]
	fld   dword ptr esi
	fmul  dword ptr edi
	fld   dword ptr esi+4
	fmul  dword ptr edi+4
	faddp
	fld   dword ptr esi+8
	fmul  dword ptr edi+8
	faddp  
	ret
endp

; input : [[vec]]
; output: st
proc Magnitude3f uses esi, vec: DWORD
	mov   esi,[vec]
	fld   dword ptr esi
	fmul  st,st
	fld   dword ptr esi+4
	fmul  st,st
	faddp
	fld   dword ptr esi+8
	fmul  st,st
	faddp
	fsqrt
	ret
endp  

; input : [[vec2]], [s]
; output: [vec1]
proc ScaleVector3f uses edi esi, vec1, vec2, s: DWORD
	mov   edi,[vec1]
	mov   esi,[vec2] 
	fld   dword ptr esi
	fmul  [s]
	fstp  dword ptr edi
	fld   dword ptr esi+4
	fmul  [s]
	fstp  dword ptr edi+4
	fld   dword ptr esi+8
	fmul  [s]
	fstp  dword ptr edi+8
	ret
endp 

; input : [[vec1]], [[vec2]], [f]
; output: [vec]
proc LerpVector3f uses edi eax ebx, vec, vec1, vec2, f: DWORD
	mov   edi,[vec]
	mov   eax,[vec1] 
	mov   ebx,[vec2] 
	fld   [f]
	fld   dword ptr eax
	fld   dword ptr ebx
	fsub  st,st1
	fmul  st,st2
	faddp
	fstp  dword ptr edi
	
	fld   dword ptr eax+4
	fld   dword ptr ebx+4
	fsub  st,st1
	fmul  st,st2
	faddp
	fstp  dword ptr edi+4
	
	fld   dword ptr eax+8
	fld   dword ptr ebx+8
	fsub  st,st1
	fmul  st,st2
	faddp
	fstp  dword ptr edi+8
	ffree st
	ret
endp

; ----- Vector4f functions -----

proc ZeroVector4f uses esi eax, vec: DWORD
	mov  esi,[vec]
	xor  eax,eax
	mov  [esi],eax
	mov  [esi+4],eax
	mov  [esi+8],eax
	mov  [esi+12],eax
	ret
endp

; input : [[vec1]],[[vec2]]
; output: [vec]
proc AddVector4f vec,vec1,vec2: DWORD
	push   edi eax ebx
	mov    edi,[vec]
	mov    eax,[vec1]
	mov    ebx,[vec2]
	movups xmm0,[eax]
	movups xmm1,[ebx]
	addps  xmm0,xmm1
	movups [edi],xmm0 
	pop    ebx eax edi
	ret
endp

; input : [[vec1]],[[vec2]]
; output: [vec]
proc SubVector4f uses edi eax ebx, vec,vec1,vec2: DWORD
	mov    edi,[vec]
	mov    eax,[vec1]
	mov    ebx,[vec2]
	movups xmm0,[eax]
	movups xmm1,[ebx]
	subps  xmm0,xmm1
	movups [edi],xmm0 
	ret
endp

; input : [[vec]]
; output: st
proc Norm4f uses esi, vec: DWORD
	mov   esi,[vec]
	fld   dword ptr esi
	fmul  st,st
	fld   dword ptr esi+4
	fmul  st,st
	faddp 
	fld   dword ptr esi+8
	fmul  st,st
	faddp  
	fld   dword ptr esi+12
	fmul  st,st
	faddp 
	ret
endp

; Quick and dirty normalize using SSE instructions without checking for negative norm
; input : [[vec2]]
; output: [vec1]
proc QuickNormalize4f uses edi esi, vec1, vec2: DWORD
	mov	 edi,[vec1]
	mov	 esi,[vec2]
	movups	 xmm0,[esi]
	movups	 xmm1,xmm0
	mulps	 xmm0,xmm0
	movups	 xmm2,xmm0
	shufps	 xmm0,xmm0,9h
	addps	 xmm0,xmm2
	shufps	 xmm2,xmm2,12h
	addps	 xmm0,xmm2

	rsqrtps  xmm0,xmm0
	mulps	 xmm1,xmm0
	movups	 [edi],xmm1 
	ret
endp

; Regular normalize
; input : [[vec2]]
; output: [vec1]
proc Normalize4f uses edi esi eax, vec1, vec2: DWORD
	mov   edi,[vec1]
	mov   esi,[vec2]
	fld   dword ptr esi
	fmul  st,st
	fld   dword ptr esi+4
	fmul  st,st
	faddp 
	fld   dword ptr esi+8
	fmul  st,st
	faddp 
	fld   dword ptr esi+12
	fmul  st,st
	faddp 
	
	fldz
	fcomp
	fnstsw ax
	sahf
	jz result
	fsqrt
	fld1
	fdivrp
  result:
	fld   st
	fmul  dword ptr esi	    
	fstp  dword ptr edi
	fld   st
	fmul  dword ptr esi+4
	fstp  dword ptr edi+4
	fld   st 
	fmul  dword ptr esi+8
	fstp  dword ptr edi+8 
	fmul  dword ptr esi+12
	fstp  dword ptr edi+12 
	ret
endp

; input : [[vec1]], [[vec2]], [f]
; output: [vec]
proc LerpVector4f uses edi eax ebx, vec, vec1, vec2, f: DWORD
	mov    edi,[vec]
	mov    eax,[vec1] 
	mov    ebx,[vec2] 
	movups xmm0,[eax]
	movups xmm1,[ebx]
	movss  xmm2,[f]
	pshufd xmm2,xmm2,0
	movups xmm3,xmm0
	subps  xmm0,xmm1
	mulps  xmm0,xmm2
	subps  xmm3,xmm0
	movups [edi],xmm3   
	ret
endp

; ----- Matrix4x4 functions -----

; input : [[mat1]], [[mat2]]
; output: [mat]
proc MultMatrix4x4 uses edi eax ebx, mat, mat1, mat2: DWORD
	mov	edi,[mat]
	mov	eax,[mat1] 
	mov	ebx,[mat2] 
	movss	xmm0,[eax]
	movups	xmm1,[ebx]
	shufps	xmm0,xmm0,0h
	movss	xmm2,[eax + 4h]
	mulps	xmm0,xmm1
	shufps	xmm2,xmm2, 0h
	movups	xmm3,[ebx + 10h]
	movss	xmm4,[eax + 8h]
	mulps	xmm2,xmm3
	shufps	xmm4,xmm4,0h
	addps	xmm0,xmm2
	movups	xmm2,[ebx + 20h]
	mulps	xmm4,xmm2
	movups	xmm5,[ebx + 30h]
	movss	xmm2,[eax + 0Ch]
	addps	xmm0,xmm4
	shufps	xmm2,xmm2,0h
	mulps	xmm2,xmm5
	addps	xmm0,xmm2
	movups	[edi],xmm0

	movss	xmm0,[eax + 1Ch]
	movss	xmm3,[eax + 18h]
	shufps	xmm0,xmm0,0h
	shufps	xmm3,xmm3,0h
	movups	xmm2,[ebx + 20h]
	mulps	xmm0,xmm5
	mulps	xmm3,xmm2
	movss	xmm4,[eax + 14h]
	addps	xmm0,xmm3
	movups	xmm1,[ebx + 10h]
	shufps	xmm4,xmm4,0h
	movss	xmm6,[eax + 10h]
	mulps	xmm1,xmm4
	shufps	xmm6,xmm6,0h
	movups	xmm2,[ebx]
	addps	xmm0,xmm1
	mulps	xmm6,xmm2
	movss	xmm3,[eax + 20h]
	addps	xmm0,xmm6
	shufps	xmm3,xmm3,0h
	movups	[edi + 10h],xmm0

	mulps	xmm2,xmm3
	movss	xmm0,[eax + 24h]
	movups	xmm4,[ebx + 10h]
	shufps	xmm0,xmm0, 0h
	movss	xmm5,[eax + 28h]
	mulps	xmm0,xmm4
	shufps	xmm5,xmm5,0h
	addps	xmm2,xmm0
	movups	xmm3,[ebx + 20h]
	movss	xmm1,[eax + 2Ch]
	mulps	xmm5,xmm3
	shufps	xmm1,xmm1,0h
	addps	xmm2,xmm5
	movups	xmm4,[ebx + 30h]
	movss	xmm6,[eax + 3Ch]
	mulps	xmm1,xmm4
	shufps	xmm6,xmm6,0h
	addps	xmm2,xmm1
	movups	[edi + 20h],xmm2

	mulps	xmm4,xmm6
	movss	xmm0,[eax + 38h]
	movups	xmm1,[ebx + 20h]
	shufps	xmm0,xmm0,0h
	movss	xmm2,[eax + 34h]
	mulps	xmm0,xmm1
	shufps	xmm2,xmm2,0h
	addps	xmm4,xmm0
	movups	xmm5,[ebx + 10h]
	movss	xmm1,[eax + 30h]
	mulps	xmm2,xmm5
	shufps	xmm1,xmm1,0h
	movups	xmm6,[ebx]
	addps	xmm4,xmm2
	mulps	xmm1,xmm6
	addps	xmm4,xmm1
	movups	[edi + 30h],xmm4   
	ret
endp

